/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.CANJaguar.SpeedReference;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

public class FRC2012Robot extends IterativeRobot {
   
    /* ----- Robot Drive ----- **/
    private final int FRONT_LEFT_JAG_CHANNEL = 1;
    private final int FRONT_RIGHT_JAG_CHANNEL = 2;
    private final int REAR_LEFT_JAG_CHANNEL = 3;
    private final int REAR_RIGHT_JAG_CHANNEL = 4;
    
    private Joystick joystickOne;
    private Joystick joystickTwo;   
    
    private double driveSensitivity = 1;
    
    // The "" at the beginning is to offset the array so the motors match up with 1,2,3, etc.
    private String[] motorNames = {"","FrontLeft", "FrontRight", "RearLeft", "RearRight", "ImpellerWheelOne", "ImpellerWheelTwo","MoveableArm"};
    
    private RobotDrive robotDrive;
    private CANJaguar frontLeft;
    private CANJaguar frontRight;
    private CANJaguar rearLeft;
    private CANJaguar rearRight; 
    
    /* ----- Drive Wheels Encoder Information ----- **/
    private double currentPosition = 0;
    private EncoderControl frontLeftEncoder;
    private EncoderControl rearLeftEncoder;
    private EncoderControl frontRightEncoder;
    private EncoderControl rearRightEncoder;

    /* ----- Impeller Wheels ----- **/
    private final int WHEEL_ONE_CHANNEL = 5;  //bottom
    private final int WHEEL_TWO_CHANNEL = 6;  //top
    private double averageWheelOneSpeed;
    private double averageWheelTwoSpeed;
    private double wheelOneThrottle = 0.0;
    private double wheelTwoThrottle = 0.0;
    private int wheelButton = 0;
    private CANJaguar wheelOne;
    private CANJaguar wheelTwo; 
    private Encoder encoderWheelOne;
    private Encoder encoderWheelTwo;
    
    /* ----- Ultrasonic Sensor ----- **/
    private final int ULTRASONIC_SENSOR_CHANNEL = 1;
    private AnalogChannel ultrasonic;
    private double averageUltrasonicDistance;
    
    /* ----- Ball Feeder ----- **/
    private final int BALL_FEEDER_CHANNEL = 1;
    private Relay ballFeeder;
    
    /* ----- Moveable Arm ----- **/
    private final int MOVEABLE_ARM_CHANNEL = 7;
    //private final int LOCKING_SERVO_CHANNEL = 1;
    //private Servo lockingServo;
    //private boolean isArmLocked;
    //private boolean previousIsLockArmButtonPressed;
    private CANJaguar moveableArm;
    //private double armDelay = 0.0;
    //private boolean hasArmDelay = false;
    
    
    /* ----- Ball Elevator ----- **/
    private final int ELEVATOR_SENSOR_CHANNEL = 1;    
    private DigitalInput elevatorSensor;
    private ButtonToggle elevatorToggle;
    private final int ELEVATOR_ARM_CHANNEL = 2;
    private Relay ballElevator;
    
    /* ----- Camera ----- **/
    private AxisCamera camera;
    
    /* ----- Misc ----- **/
    private DriverStationLCD dsLCD;
    
    
    private int codesPerRev = 250, maxRetries;
    private CANJaguar initJag(int motor)
    {
        return initJag(motor, CANJaguar.NeutralMode.kBrake);
    }
    private CANJaguar initJag(int motor, CANJaguar.NeutralMode neutralMode)
    {
        maxRetries = 0;
        CANJaguar jag = null;
        for (maxRetries = 0; jag == null && maxRetries < 10; maxRetries++)
        {
            try
            {
                jag = new CANJaguar(motor);
            }
            catch (Exception e)
            {               
                System.out.println("CANJaguar(" + motor + ") \"" + motorNames[motor] + "\" error " + e.getMessage());
                Timer.delay(.1);
            }
        }
        if (jag == null)
        {
            System.out.println("CANJaguar(" + motor + ") \"" + motorNames[motor] + "\" did not initialize, cannot drive");
        }
        else
        {
            try 
            {
                jag.configEncoderCodesPerRev(codesPerRev);
                jag.setSpeedReference(SpeedReference.kEncoder);
                jag.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
                jag.configNeutralMode(neutralMode);
            } catch (CANTimeoutException ex) 
            {
                System.out.println("CANJaguar(" + motor + ") \"" + motorNames[motor] + "\" error configuring encoder: " + ex.getMessage());
            }
        }
        return jag;
    }
    
    private double pow(double num1, int power)
    {
        double result = num1;
        for (int i = 1; i < power; i++)
        {
            result *= result;
        }
        return result;
    }
    
    private void printLine(int lineNum, String message)
    {
        printLine(lineNum, 1, message);        
    }
    private void printLine(int lineNum, int column, String message)
    {
        switch (lineNum)
        {
            case 1:
                dsLCD.println(DriverStationLCD.Line.kMain6, column, message); break;
            case 2:
                dsLCD.println(DriverStationLCD.Line.kUser2, column, message); break;
            case 3:
                dsLCD.println(DriverStationLCD.Line.kUser3, column, message); break;
            case 4:
                dsLCD.println(DriverStationLCD.Line.kUser4, column, message); break;
            case 5:
                dsLCD.println(DriverStationLCD.Line.kUser5, column, message); break;
            case 6:
                dsLCD.println(DriverStationLCD.Line.kUser6, column, message); break;
        }
        dsLCD.updateLCD();
    }
    
    private void updateEncoderData()
    {
        Watchdog.getInstance().feed();
        frontLeftEncoder.Update();
        rearLeftEncoder.Update();
        frontRightEncoder.Update();
        rearRightEncoder.Update();
        Watchdog.getInstance().feed();
    }
    
    private double getThrottle(Joystick joystick)
    {
        if (joystick == null)
            return 0;
        
        return Math.abs(joystick.getThrottle() - 1.0) / 2.0;
    }
    
    private double ultrasensorVoltageTotal;
    private double ultrasensorAverageCount = 1;
    private void updateUltrasonicSensorDistance()
    {
        Watchdog.getInstance().feed();
        ultrasensorVoltageTotal += ultrasonic.getAverageVoltage();
        ultrasensorAverageCount++;
        if (ultrasensorAverageCount % 8 == 0) {
            averageUltrasonicDistance = ultrasensorVoltageTotal / ultrasensorAverageCount * 2000 / 23 - 16 / 23;
            ultrasensorAverageCount = 1;
            ultrasensorVoltageTotal = 0.0;
        }
        Watchdog.getInstance().feed();
    }
    private double wheelOneSpeedTotal;
    private double wheelTwoSpeedTotal;
    private double wheelAverageCount = 1;
    private void updateImpellerWheelSpeed()
    {
        Watchdog.getInstance().feed();
        try {
            
            wheelOneSpeedTotal += wheelOne.getSpeed();
            wheelTwoSpeedTotal += wheelTwo.getSpeed();
        } catch (Exception ex)
        {
            printLine(6, "Error getting impeller wheel speed: " + ex.getMessage());
        }
        wheelAverageCount++;
        if (wheelAverageCount % 8 == 0) {
            averageWheelOneSpeed = wheelOneSpeedTotal / wheelAverageCount;
            averageWheelTwoSpeed = wheelTwoSpeedTotal / wheelAverageCount;
            wheelOneSpeedTotal = 0;
            wheelTwoSpeedTotal = 0;
            wheelAverageCount = 1;
        }
        Watchdog.getInstance().feed();
    }
    private double getImpellerWheelSpeed(double distance)
    {
        //todo: 
        return .5;
    }
    
    private void controlElevator()
    {
        Watchdog.getInstance().feed();
        if (joystickTwo.getRawButton(3))
        {
            elevatorToggle.SetToggle();
        }
        else
        {
            elevatorToggle.ResetButton();
        }
        
        if (elevatorToggle.toggle)
        {
            ballElevator.set(Relay.Value.kOn);
        }
        else
        {
            if (elevatorSensor.get())
                ballElevator.set(Relay.Value.kOn);
            else
                ballElevator.set(Relay.Value.kOff);
        }
        Watchdog.getInstance().feed();
    }
    private void controlMoveableArm()
    {
        Watchdog.getInstance().feed();
        try
        {
            if (!moveableArm.getReverseLimitOK() && joystickTwo.getRawButton(4))
            {
                moveableArm.setX(.60);
            }            
            else if (!moveableArm.getForwardLimitOK() && joystickTwo.getRawButton(1))
            {
                moveableArm.setX(-.60);
            }
            else
            {
                moveableArm.setX(0);
            }
            
//            //If the down arm limit switch is pressed AND we are not trying to go back up...
//            if (!moveableArm.getReverseLimitOK() && joystickTwo.getRawButton(4))
//            {
////                if (!isArmLocked && !hasArmDelay)
////                {
////                    hasArmDelay = true;
////                    armDelay = System.currentTimeMillis();
////                }
////                if (hasArmDelay && (System.currentTimeMillis() - armDelay) > 500)
////                {
////                    lockingServo.set(.65);
////                    if (lockingServo.get() >= .60)
////                        isArmLocked = true;
////                }
//            }
//            
//            // If the down arm limit switch is not pressed, we are not locked.
//            if (moveableArm.getReverseLimitOK())                
//                isArmLocked = false;
//                
//            // If we are trying to lower the arm and it is not locked...
//            if (joystickTwo.getRawButton(1) && !isArmLocked)
//                moveableArm.setX(-.60);
//            // Otherwise, if  we are trying to go up, and...
//            else if (joystickTwo.getRawButton(4))
//            {
//                // The arm is locked OR the locking servo is in it's "unlocked" position
//                if (isArmLocked || lockingServo.get() > .185)
//                {
//                    // The arm is not locked
//                    isArmLocked = false;
//                    hasArmDelay = false;
//                    // Make sure the locking servo is unlocked.
//                    lockingServo.set(.18);
//                }
//                // Otherwise
//                else
//                    // Move the arm up.
//                    moveableArm.setX(.60);
//            }            
//            //Otherwise
//            else 
//                // Don't move the arm.
//                moveableArm.setX(0);
        }
        catch (CANTimeoutException ex)
        {
            printLine(6, "Error setting moveable arm: " + ex.getMessage());
        }
        Watchdog.getInstance().feed();
    }

    public void robotInit()
    {
        Watchdog.getInstance().setExpiration(0.4);
        Watchdog.getInstance().kill();
        
        joystickOne = new Joystick(1);
        joystickTwo = new Joystick(2);
        
        ultrasonic = new AnalogChannel(ULTRASONIC_SENSOR_CHANNEL);
        
        elevatorSensor = new DigitalInput(ELEVATOR_SENSOR_CHANNEL);
        elevatorToggle = new ButtonToggle(false, false);
        
        moveableArm = initJag(MOVEABLE_ARM_CHANNEL);
        //lockingServo = new Servo(LOCKING_SERVO_CHANNEL);
        //lockingServo.set(.2);
        
        frontLeft = initJag(FRONT_LEFT_JAG_CHANNEL);
        frontRight = initJag(FRONT_RIGHT_JAG_CHANNEL);
        rearLeft = initJag(REAR_LEFT_JAG_CHANNEL);
        rearRight = initJag(REAR_RIGHT_JAG_CHANNEL);
        
        frontLeftEncoder = new EncoderControl(frontLeft);
        frontRightEncoder = new EncoderControl(frontRight);
        rearLeftEncoder = new EncoderControl(rearLeft);
        rearRightEncoder = new EncoderControl(rearRight);
        
        robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
        
        ballFeeder = new Relay(BALL_FEEDER_CHANNEL);
        ballFeeder.setDirection(Relay.Direction.kBoth);
        
        ballElevator = new Relay(ELEVATOR_ARM_CHANNEL);
        ballElevator.setDirection(Relay.Direction.kForward);
        
        wheelOne = initJag(WHEEL_ONE_CHANNEL, CANJaguar.NeutralMode.kCoast);
        wheelTwo = initJag(WHEEL_TWO_CHANNEL, CANJaguar.NeutralMode.kCoast);        
        
        //camera = AxisCamera.getInstance("10.32.77.11");
        //camera.writeResolution(AxisCamera.ResolutionT.k640x480);
        //camera.writeCompression(0);
        //camera.writeBrightness(50);
        //pan = new Servo(PAN_CHANNEL);
        //tilt = new Servo(TILT_CHANNEL);
        
        dsLCD = DriverStationLCD.getInstance();
        for (int i = 0; i < 7; i++)
            printLine(i, "                                         ");
        flushJoystickInput(joystickOne);
    }
    
    public void flushJoystickInput(Joystick joy)
    {
        boolean buttonFlush = false;        
        while (!buttonFlush)
        {
            buttonFlush = true;
            for (int buttonIdx = 1; buttonIdx <= 12; buttonIdx++)
            {
                if (joy.getRawButton(buttonIdx))
                    buttonFlush = false;
            }
        }
    }
    
    public void sensorUpdate()
    {
        updateEncoderData();
        updateUltrasonicSensorDistance();
        updateImpellerWheelSpeed();
        Watchdog.getInstance().feed();
    }
    
    public void autonomousInit()
    {        
        try {
            frontLeft.configNeutralMode(CANJaguar.NeutralMode.kCoast);
            frontRight.configNeutralMode(CANJaguar.NeutralMode.kCoast);
            rearLeft.configNeutralMode(CANJaguar.NeutralMode.kCoast);
            rearRight.configNeutralMode(CANJaguar.NeutralMode.kCoast);
        } catch (CANTimeoutException ex) {
            System.out.println("Error in the autonomous init: " + ex.getMessage());
        }
        autonomousState = 0;
        startMillisecondCount = System.currentTimeMillis();
    }
    public void teleopInit()
    {
        try {
            frontLeft.configNeutralMode(CANJaguar.NeutralMode.kBrake);
            frontRight.configNeutralMode(CANJaguar.NeutralMode.kBrake);
            rearLeft.configNeutralMode(CANJaguar.NeutralMode.kBrake);
            rearRight.configNeutralMode(CANJaguar.NeutralMode.kBrake);
        } catch (CANTimeoutException ex) {
            System.out.println("Error in the teleop init: " + ex.getMessage());
        }
    }
    
    private int autonomousState;
    private long startMillisecondCount = 0;
    public void autonomousPeriodic()
    {
        Watchdog.getInstance().feed();
        
        sensorUpdate();
        
        int ballFeederCycleTime = 1700;
        int ballRollOntoHoopTime = 2700;
        int wheelAccelerationTime = 4000;
        
        double wheelOneSetCenterWheelSpeed = .36;
        double wheelTwoSetCenterWheelSpeed = .15;
        
        double driveRotateTime = 1200;
        double driveBackwardTime = 1500;
        
        double wheelOneSetWheelSpeed = .34;
        double wheelTwoSetWheelSpeed = .21;
        
        printLine(1, "Wheel1: " + averageWheelOneSpeed + (averageWheelOneSpeed <= 0.0001 ? "              " : ""));
        printLine(2, "Wheel2: " + averageWheelTwoSpeed + (averageWheelTwoSpeed <= 0.0001 ? "              " : ""));
        printLine(3, "Ultrasonic: " + averageUltrasonicDistance + (averageUltrasonicDistance <= 0.001 ? "               " : ""));
        try {
            printLine(4, "Distance: " + frontLeft.getPosition() + " ");
        } catch (CANTimeoutException ex) {
            
        }
        
        
        try {
            wheelOne.setX(wheelOneSetWheelSpeed);
            wheelTwo.setX(wheelTwoSetWheelSpeed);
                
            if (autonomousState == 0)
            {
                if (System.currentTimeMillis() - startMillisecondCount >= (wheelAccelerationTime - 250))
                    robotDrive.mecanumDrive_Cartesian(0, -.15, 0, 0);
                else    
                    robotDrive.mecanumDrive_Cartesian(0, -.25, 0, 0);
                wheelOne.setX(wheelOneSetWheelSpeed);
                wheelTwo.setX(wheelTwoSetWheelSpeed);
                if (System.currentTimeMillis() - startMillisecondCount >= wheelAccelerationTime) // Wait for impeller wheels to get up to speed
                {
                    autonomousState++;
                    startMillisecondCount = System.currentTimeMillis();
                    
                }
            }
            else if (autonomousState == 1)
            {
                if (System.currentTimeMillis() - startMillisecondCount >= 1000)
                {
                    autonomousState++;
                    startMillisecondCount = System.currentTimeMillis();
                    ballFeeder.set(Relay.Value.kForward);
                }
            }
            else if (autonomousState == 2) // Wait until ball feeder makes one revolution
            {
                if (System.currentTimeMillis() - startMillisecondCount >= ballFeederCycleTime)
                {
                    ballFeeder.set(Relay.Value.kOff);
                    startMillisecondCount = System.currentTimeMillis();
                    autonomousState++;
                }
            } 
            else if (autonomousState == 3) // Wait for ball feeder to hold ball
            {
                if (System.currentTimeMillis() - startMillisecondCount >= ballRollOntoHoopTime) 
                {
                    ballFeeder.set(Relay.Value.kForward);
                    startMillisecondCount = System.currentTimeMillis();
                    autonomousState++;
                }
            }
            else if (autonomousState == 4)
            {
                if (System.currentTimeMillis() - startMillisecondCount >= ballFeederCycleTime)
                {
                    ballFeeder.set(Relay.Value.kOff);
                    wheelOne.setX(0);
                    wheelTwo.setX(0);
                    
                    startMillisecondCount = System.currentTimeMillis();
                    autonomousState++;
                }
            }
            /*
            else if (autonomousState == 5)
            {
                 if (System.currentTimeMillis() - startMillisecondCount >= driveBackwardTime)
                 {
                     startMillisecondCount = System.currentTimeMillis();
                     autonomousState++;
                 } 
                 else 
                     robotDrive.arcadeDrive(0, .7);
            }
            else if (autonomousState == 6)
            {
                 if (System.currentTimeMillis() - startMillisecondCount >= driveRotateTime)
                 {
                     robotDrive.arcadeDrive(0, 0);
                     autonomousState++;
                 } 
                 else 
                     robotDrive.arcadeDrive(.7, 0);
            }
             */
             
        } catch (Exception ex) {
            printLine(6, "Error during autonomous mode: " + ex.getMessage());
        }
    }

    public void teleopPeriodic()
    {
        Watchdog.getInstance().feed();
        
        // Update the sensors
        sensorUpdate();
       
        //drive
        if (joystickOne.getRawButton(2))
            driveSensitivity = .3;
        else
            driveSensitivity = 1;
        //double x = driveSensitivity * joystickOne.getTwist();
//        int expo = 3;
//        double driveTwist = 0;
//        if (x < 0)
//            driveTwist = -pow(-x, expo);
//        if (x >= 0)
//            driveTwist = pow(x, expo);
        
        
        
        robotDrive.mecanumDrive_Cartesian(driveSensitivity * joystickOne.getX(), driveSensitivity * joystickOne.getY(), driveSensitivity * joystickOne.getTwist(), 0);

        //elevator
        controlElevator();
        
        //moveable arm
        controlMoveableArm();
        
        //ball feeder
        if (joystickOne.getTrigger())
            if (joystickOne.getRawButton(7))
            {
                ballFeeder.set(Relay.Value.kReverse);
            }
            else
            {
                ballFeeder.set(Relay.Value.kForward);
            }
        else
            ballFeeder.set(Relay.Value.kOff);
             
        //impeller wheels
        try {
            double slider = getThrottle(joystickOne);
            if (joystickTwo.getRawButton(7))
            {
               wheelButton = 0;
            }
            if (joystickTwo.getRawButton((6)))
            {
                wheelButton = 1;
            }
            if (joystickTwo.getRawButton(5))
            {
                wheelButton = 2;
            }
            if (wheelButton == 0)
            {
                wheelOneThrottle = 0;
                wheelTwoThrottle = 0;
            }
            if (wheelButton == 1)
            {
                wheelOneThrottle = .34 + (.5 - .34) * slider;                
                wheelTwoThrottle = .21 + (.15 - .21) * slider;                
            }
            if (wheelButton == 2)
            {                
                wheelOneThrottle = .5 + (.5) * (slider / 10);
                wheelTwoThrottle = .15 + (.15) * (slider / 10);                
            }
            
            
            
            wheelOne.setX(wheelOneThrottle);
            wheelTwo.setX(wheelTwoThrottle); // /3.5
            //printLine(1, "ElevatorSensor: " + (elevatorSensor.get() ? "true " : "false") + " ");
            printLine(1, "Slider(1): " + wheelOneThrottle + (wheelOneThrottle <= 0.001 ? "       " : ""));
            printLine(2, "Slider(2): " + wheelTwoThrottle + (wheelTwoThrottle <= 0.001 ? "       " : ""));
            printLine(3, "Wheel1: " + averageWheelOneSpeed + (averageWheelOneSpeed <= 0.0001 ? "       " : ""));
            printLine(4, "Wheel2: " + averageWheelTwoSpeed + (averageWheelTwoSpeed <= 0.0001 ? "       " : ""));
            printLine(5, "Ultrasonic: " + averageUltrasonicDistance + (averageUltrasonicDistance <= 0.001 ? "        " : ""));
        } catch (Exception ex)
        {
            printLine(6, "Error setting impeller wheel speed: " + ex.getMessage());
        }
        //previousIsLockArmButtonPressed = joystickOne.getRawButton(3);
        Watchdog.getInstance().feed();
    }
    
}
